package com.rockwell.robot2015.model.states.running.sequence;

import org.opencv.core.Point;

import com.rockwell.robot2015.model.MazeType;
import com.rockwell.robot2015.model.Robot;
import com.rockwell.robot2015.model.detectors.IDetector;
import com.rockwell.robot2015.model.states.IState;
import com.rockwell.robot2015.model.states.IStateMachine;
import com.rockwell.robot2015.model.states.running.RobotRunningStateMachine;

public abstract class RobotRunningSequence implements IStateMachine {
	public static final int CAMERA_DISTANCE = 200; //250;
	
	protected RobotRunningStateMachine m_running;
	
	protected IState m_currState, m_nextState, m_initialState;
	
	public RobotRunningSequence(RobotRunningStateMachine running, IState initialState) {
		m_running = running;
		m_initialState = initialState;
	}
	
	public abstract void start();

	public abstract boolean succeeded();
	
	public abstract boolean failed();
	
	@Override
	public final void move() {
		if (m_nextState != null) {
			changeState(m_nextState);
			m_nextState = null;
		}
		
		m_currState.move(this);
	}

	@Override
	public void changeState(IState state) {
		if (!state.equals(m_currState)) {
			if (m_currState != null) {
				m_currState.exit(this);
			}
			
			m_currState = state;
			m_currState.enter(this);
		}
	}

	@Override
	public void reset() {
		m_nextState = m_initialState;
	}
	
	@Override
	public IState getState() {
		return m_currState;
	}

	@Override
	public Robot robot() {
		return m_running.robot();
	}

	@Override
	public void waitForResume() {
		m_running.waitForResume();
	}
	
	public static int getOrientation(Robot robot, IDetector detector) {
		Point drift = detector.getDrift();

		String log = String.format("Target drift (%f, %f)", drift.x, drift.y);
		robot.log(log, 4);
		
		Point center = detector.getCenter();
		log = String.format("Target center (%f, %f)", center.x, center.y);
		robot.log(log, 4);
		
		double degree = Math.toDegrees(Math.atan2(drift.y, drift.x + CAMERA_DISTANCE));

		log = String.format("Target angle (%f)", degree/1.5);
		robot.log(log, 4);
		
		if (degree == Double.NaN) {
			return 0;
		}
		else {
			return (int)(degree/1.5);
		}
	}
	
	/**
	 * Indicate the alignment of the treasure
	 * @author XWu1
	 *
	 */
	public enum TreasureAlignment {
		Lost,
		Aligned,
		NotAligned
	}
	
	public static boolean detectTreasure(Robot robot) {
		IDetector treasure = robot.getTreasureDetector();
		IDetector maze1 = robot.getMaze1Detector();
		IDetector maze2 = robot.getMaze2Detector();
		IDetector maze3 = robot.getMaze3Detector();
		IDetector outerWall = robot.getOuterWallDetector();
		
		if (!treasure.isPresent())
			return false;
		
		if (maze1.isPresent()) {
			return maze1.getCenter().x < treasure.getCenter().x;
		}
		
		if (maze2.isPresent()) {
			return maze2.getCenter().x < treasure.getCenter().x;
		}
		
		if (maze3.isPresent()) {
			return maze3.getCenter().x < treasure.getCenter().x;
		}
		
		if (outerWall.isPresent()) {
			return outerWall.getCenter().x < treasure.getCenter().x;
		}
		
		return true;
	}
	
	public static TreasureAlignment getTreasureAlignment(Robot robot, int threshold) {
		IDetector detector = robot.getTreasureDetector();
		
		if (detector.isPresent()) {
			int degree = getOrientation(robot, detector);

			if (Math.abs(degree) > threshold) {
				return TreasureAlignment.NotAligned;
			} else {
				return TreasureAlignment.Aligned;
			}
		}
		else {
			return TreasureAlignment.Lost;
		}
	}
	
	public static MazeType checkMaze(IStateMachine machine) {
		IDetector detector1 = machine.robot().getMaze1Detector();
		IDetector detector2 = machine.robot().getMaze2Detector();
		IDetector detector3 = machine.robot().getMaze3Detector();

		if (detector1.isPresent()) {
			return MazeType.Maze1;
		} else if (detector2.isPresent()) {
			return MazeType.Maze2;
		} else if (detector3.isPresent()) {
			return MazeType.Maze3;
		} else {
			return MazeType.None; // no maze! error!
		}
	}

	public static IDetector getMazeDetector(Robot robot) {
		IDetector mazeDetector = null;
		
		if (robot.getMaze1Detector().isPresent()){
			//robot.log("Find maze1", 4);
			mazeDetector = robot.getMaze1Detector();
		}
		else if (robot.getMaze2Detector().isPresent()){
			//robot.log("Find maze2", 4);
			mazeDetector = robot.getMaze2Detector();
		}
		else if (robot.getMaze3Detector().isPresent() && robot.shouldTryMaze3()){
			//robot.log("Find maze3", 4);
			mazeDetector = robot.getMaze3Detector();
		}
		
		return mazeDetector;
	}
	
	public static boolean detectEntrance(Robot robot) {
		IDetector detector = robot.getMazeEntranceGuideDetector();
		return detector.isPresent();
	}
}
